#coding=utf-8
'''
    双足机器人控制APP
'''
import threading
import time
import yaml

#   算法
import numpy as np

import proc_data.recorder
#   数据
from controller import geometric,kinematic,physical
from controller.params import Params
#   网络
from interfaces.udp import udp_data_bridge
from interfaces.video import mjpeg
#   界面
from gui import dual_leg_panel
#   定义
from definations.definations import *
#   控制
from PySide2.QtWidgets import QApplication
from PySide2.QtCore import QTimer
#   数据录制
from proc_data import recorder

class APP:

    def __init__(self):
        '''
        应用程序
        '''
        self.LoadConfig()

        self.InitContrller()
        self.InitPanel()
        self.InitInterfaces()
        self.StartSync()
        #self.CalibSensors()
        #self.OutputVoice('你好，我是呆果机器人，有什么需要帮助的呢？',-1,1)
    def LoadConfig(self):
        '''
        载入配置信息
        :return:
        '''

        with open('config/config.yaml',encoding='utf-8') as file:
            self.config=yaml.load(file,Loader=yaml.FullLoader)
        file.close()
        with open('config/adjustable_params.yaml',encoding='utf-8') as file:
            Params.controlParams=yaml.load(file,Loader=yaml.FullLoader)
        file.close()

    def DumpConfig(self,bundle):
        '''
        存储配置数据
        :param bundle:
        :return:
        '''
        self.config.update(bundle)
        with open('config/config.yaml','w', encoding='utf-8') as file:
            yaml.dump(self.config,file,default_flow_style=False)
        file.close()
    def DumpAdjConfig(self,bundle):
        Params.controlParams.update(bundle)
        with open('config/adjustable_params.yaml', 'w', encoding='utf-8') as file:
            yaml.dump(Params.controlParams, file, default_flow_style=False)
        file.close()

    def InitContrller(self):
        '''
        初始化控制器
        :return:
        '''
        self.lFoot=geometric.FootTerminal(0,self.config['hip_width']/2,0.0,name="左脚",size=self.config['foot_size'])
        self.rFoot=geometric.FootTerminal(0,-self.config['hip_width']/2,0.0,name="右脚",size=self.config['foot_size'])
        self.base = geometric.BaseLink(0,0,np.sum(self.config['bone_length']),name="脑袋",weight=self.config['head_weight'])    #初始状态确定base_link的位置

        #   骨骼(AppleChain)
        self.lChain=geometric.AppleChain(baseEntity=self.base,terminalEntity=self.lFoot,basedockerPos=np.array([0,self.config['hip_width']/2,0.0]),
                                         boneLength=self.config['bone_length'],
                                         boneWeights=self.config['bone_weights'],
                                         thresholds=self.config['joint_thresholds'])
        self.rChain = geometric.AppleChain(baseEntity=self.base, terminalEntity=self.rFoot,basedockerPos=np.array([0,-self.config['hip_width']/2, 0.0]),
                                           boneLength=self.config['bone_length'],
                                           boneWeights=self.config['bone_weights'],
                                           thresholds=self.config['joint_thresholds'])
        self.lChain.LoadStaticErr(self.config['err_static']['l'])
        self.rChain.LoadStaticErr(self.config['err_static']['r'])

        #   物理映射
        self.physical = physical.Physical(rootObject=self)
        #   控制器
        self.kinematic = kinematic.Kinematic_Controller(self)



    def InitInterfaces(self):
        '''
        初始化接口
        :return:
        '''
        #   UDP网络接口
        self.netBridge=udp_data_bridge.UDP_Data_Bridge(parent=self,lFoot=self.lFoot,rFoot=self.rFoot,base=self.base,lChain=self.lChain,rChain=self.rChain,\
                                                       ip_console=self.config['ip_console'],ip_lfoot=self.config["ip_lfoot"],ip_rfoot=self.config["ip_rfoot"],\
                                                       port_base=self.config["port_base"],port_lfoot=self.config["port_lfoot"],port_rfoot=self.config["port_rfoot"],\
                                                       port_voiceout=self.config["port_voiceout"],port_jointcontrol=self.config["port_jointcontrol"],\
                                                       port_stampync=self.config["port_stampsync"])
        self.netBridge.StampSync()
        #   TCP/HTTP视频网络接口
        self.videoBridge=mjpeg.MJPEG_Bridge(url=self.config["vision_url"],img_Size=self.config["vision_framesize"])
        self.videoBridge.Start()
    def InitPanel(self):
        '''
        控制面板
        :return:
        '''
        self.panel = dual_leg_panel.Panel(rootObject=self)
        self.panel.ConfigPanel()

    def StartSync(self):
        '''
        开启高，中，低循环时钟
        :return:
        '''
        self.Log('程序启动')
        self.timer_Fast=QTimer()
        self.timer_Fast.timeout.connect(self.SyncFast)
        self.timer_Fast.start(int(1000*(1.0/Params.controlParams['freq_high'])))
        self.timer_medium = QTimer()
        self.timer_medium.timeout.connect(self.SyncMedium)
        self.timer_medium.start(int(1000*(1.0/Params.controlParams['freq_med'])))
        self.timer_Slow = QTimer()
        self.timer_Slow.timeout.connect(self.SyncSlow)
        self.timer_Slow.start(int(1000*(1.0/Params.controlParams['freq_low'])))

    def SyncFast(self):
        '''
        快速实时更新的数据
        :return:
        '''
        '''
            传感器数据占用资源小，需要实时查看，所以要快速更新
        '''
        #   更新足底传感器数据
        self.panel.pressureL.UpdateData(self.lFoot.pressure)
        self.panel.pressureR.UpdateData(self.rFoot.pressure)
        self.panel.ZMPL.UpdateData(self.lFoot.pressCenter[0],self.lFoot.pressCenter[1],self.lFoot.pressCenterQueue[-1][0]-self.lFoot.pressCenterQueue[-3][0],self.lFoot.pressCenterQueue[-1][1]-self.lFoot.pressCenterQueue[-3][1])
        self.panel.ZMPR.UpdateData(self.rFoot.pressCenter[0], self.rFoot.pressCenter[1], self.rFoot.pressCenterQueue[-1][0] - self.rFoot.pressCenterQueue[-3][0],
                                   self.rFoot.pressCenterQueue[-1][1] - self.rFoot.pressCenterQueue[-3][1])
        #   更新姿态数据
        self.panel.rollCanvas.UpdateData(self.lFoot.rotationMonitored[0], self.rFoot.rotationMonitored[0],
                                         self.base.rotationMonitored[0])
        self.panel.pitchCanvas.UpdateData(self.lFoot.rotationMonitored[1], self.rFoot.rotationMonitored[1],
                                          self.base.rotationMonitored[1])
    def SyncMedium(self):
        '''
        中速更新数据
        用于界面显示等
        :return:
        '''
        #   控制循环
        # self.kinematic.CtrlCycle()
        # #   同步硬件
        # if self.panel.ui_Main.autoCheck.isChecked():
        #     self.physical.Sync()
        #   渲染3D机器人状态
        self.panel.modelWidget.RenderRobot()
        # #   更新原始图像数据
        self.panel.video.Update(self.videoBridge.image)
        #   更新网络延迟数据
        bat, amp = self.base.GetBattery()
        temp = self.base.GetTemperature()
        text = "频率: 左脚<font color=\"yellow\">{:02d}Hz  </font>\t右脚 <font color=\"yellow\">{:02d}Hz</font>\t头部 <font color=\"yellow\">{:02d}Hz</font>\t|\tPing:左脚<font color=\"green\">{:03d}ms\t</font>   右脚 <font color=\"green\">{:03d}ms</font>\t头部 <font color=\"green\">{:03d}ms</font>".format(
            self.rFoot.latency.rate, self.lFoot.latency.rate, self.base.latency.rate, self.rFoot.latency.val, self.lFoot.latency.val, self.base.latency.val)
        self.panel.latencyLabel.setText(text)
        self.panel.batteryLabel.SetBattery(bat, amp, temp)
    def SyncSlow(self):
        '''
        慢速更新数据
        :return:
        '''
        self.netBridge.StampSync(rate=Params.controlParams['freq_high'])

    ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
        功能函数
    '''''

    def SetFilterLevel(self,level=0):
        '''
        设置滤波器滤波等级
        :param level: 0：无滤波 1：中值滤波 2：中值+卡尔曼
        :return:
        '''
        self.base.filterLevel=level
        self.lFoot.filterLevel=level
        self.rFoot.filterLevel=level

    def ReleaseJoints(self):
        '''
        释放关节
        :return:
        '''
        self.netBridge.ReleaseJoints()

    def InitJoints(self):
        '''
        手动同步关节数据
        :return:
        '''
        self.kinematic.ctrlState = STATE.STATE_MANUAL
        time.sleep(0.1)
        self.netBridge.SyncJoint([0,0,0,0,0,0],[0,0,0,0,0,0])

    def OutputVoice(self,data,speed=0,volume=1,timbre=0,mood=0):
        '''
        输出语音
        :param data:
        :return:
        '''
        self.netBridge.VoiceOut(data,speed,volume,timbre,mood)

    def CalibSensors(self):
        '''
        传感器校准以及自我诊断
        :return:
        '''
        time.sleep(1)
        self.lFoot.CalibrateSensor()
        self.rFoot.CalibrateSensor()
        self.base.CalibrateSensor()


    def Log(self,text,level='info'):
        '''
        写入日志
        :param text:
        :param level:
        :return:
        '''
        try:
            self.panel.logEdit.PushLog(text,level)
        except:
            pass

    def SyncHardware(self,l,r,h=0):
        '''
        同步硬件
        :param l:
        :param r:
        :return:
        '''
        self.netBridge.SyncJoint(l,r,h,t=0.0)


if __name__=='__main__':
    app = QApplication([])
    mainActivity=APP()
    app.exec_()